/*
 -------------------------------------------------------------------------------
 flightDataLogger.cpp
 
 This program is used to record flight data.
 
 Initially created by Rowland O'Flaherty on 1/10/12.
 
 Version
 1.0 _ Rowland
     Initial
 1.1 - Daniel
     For Pro Micro board
 1.2 - Rowland
     Added code/comments to choose between Uno or ProMirco board
 -------------------------------------------------------------------------------
 */

//------------------------------------------------------------------------------
// Includes and Defines
//------------------------------------------------------------------------------
#include <Arduino.h>
#include <SoftwareSerial.h>
#include <Wire.h>
#include <BMP085.h>
#include <ITG3200.h>
#include <ADXL345.h>
#include <HMC5843.h>
#include <NMEAGPS.h>
#include <Servo.h>



//------------------------------------------------------------------------------
// Helper Function Declarations
//------------------------------------------------------------------------------


//------------------------------------------------------------------------------
// Global Varible Declarations
//------------------------------------------------------------------------------

// Flags
bool use_serial = false;
bool use_xBee = true;

bool use_gps = true;
bool use_accel = true;
bool use_baro = true;
bool use_gyro = true;
bool use_mag = true;
bool use_servos = true;


// Logging Constants and Variables
int ts = 500; // [ms] Sampling time
int cnt = 0; // Sample count

// ++++++++++++++++++++++++++++++++++
// Arduino Pro Micro Constants and Variables
// ++++++++++++++++++++++++++++++++++
int ledPin = 17;
int serialBAUD = 9600;

// ++++++++++++++++++++++++++++++++++
// XBee Constants and Variables
// ++++++++++++++++++++++++++++++++++
int xBeeRX = 5;
int xBeeTX = 4;
int xBeeBAUD = 9600;
SoftwareSerial xBeeSerial(xBeeRX, xBeeTX);

// ++++++++++++++++++++++++++++++++++
// GPS Sensor Constants and Variables
// ++++++++++++++++++++++++++++++++++
int gpsRX = 8;
int gpsTX = 9;
NMEAGPS gps(gpsRX,gpsTX);

// +++++++++++++++++++++++++++++++++++++++
// baro Sensor Constants and Variables
// +++++++++++++++++++++++++++++++++++++++
BMP085 baro;

// ++++++++++++++++++++++++++++++++++++++++++++
// Accelerometer Sensor Constants and Variables
// ++++++++++++++++++++++++++++++++++++++++++++
ADXL345 accel;

// +++++++++++++++++++++++++++++++++++
// Gyro Sensor Constants and Variables
// +++++++++++++++++++++++++++++++++++
ITG3200 gyro;

// +++++++++++++++++++++++++++++++++++++++++++
// Magnetometer Sensor Constants and Variables
// +++++++++++++++++++++++++++++++++++++++++++
HMC5843 mag;

// +++++++++++++++++++++++++++++
// Servo & Throttle Constants and Variables
// +++++++++++++++++++++++++++++
int rudderRef, elevatorRef, throttleRef;
//Servo servoRudder;
//Servo servoElevator;

// Servo pins
//int servoRudderDigitalOut = 6;
//int servoElevatorDigitalOut = 7;
int rudderAnalogIn = 0;
int elevatorAnalogIn = 1;

// Throttle pins
int throttleAnalogIn = 3;

//------------------------------------------------------------------------------
// Setup Function - Initializes Arduino
//------------------------------------------------------------------------------
void setup() {
    delay(3000);
    
    // Initialize Arduino Pro Micro
    pinMode(ledPin, OUTPUT);
    if(use_serial) {
        Serial.begin(serialBAUD); //This pipes to the serial monitor
    }
    
    // Initialize XBee
    if(use_xBee) {
        xBeeSerial.begin(xBeeBAUD);
    }
    
    // Initialize GPS
    if(use_gps) {
        gps.initialize();
    }

    // Initialize accelerometer
    if(use_accel) {
        accel.initialize();
    
        //set activity/ inactivity thresholds(0-255)
        accel.setActivityThreshold(75); //62.5mg per increment
        accel.setInactivityThreshold(75); //62.5mg per increment
        accel.setTimeInactivity(10); // how many seconds of no activity is inactive?
    
        //look of activity movement on this axes - 1 == on; 0 == off 
        accel.setActivityX(1);
        accel.setActivityY(1);
        accel.setActivityZ(1);
    
        //look of inactivity movement on this axes - 1 == on; 0 == off
        accel.setInactivityX(1);
        accel.setInactivityY(1);
        accel.setInactivityZ(1);
    }

    // Initialize baro sensor
    if(use_baro) {
        baro.initialize();
        baro.calibrateWithAlt(0);
        baro.setOSS(3);
    }

    // Initialize gyro
    if(use_gyro) {
        gyro.initialize();
    }

    // Initialize magnetometer
    if(use_mag) {
        mag.initialize();
        mag.setSamplingRate(4); // 10Hz
        mag.setGain(0); // 0.7Ga
    }
    
    // Setup servos
    rudderRef = analogRead(rudderAnalogIn);
    elevatorRef = analogRead(elevatorAnalogIn);
    throttleRef = analogRead(throttleAnalogIn);
    
    //if(use_servos) {
    //    servoRudder.attach(servoRudderDigitalOut);
    //    servoElevator.attach(servoElevatorDigitalOut);
    //}


    // +++++++++++++++++++++++++++++
    // Print header for data logging
    // +++++++++++++++++++++++++++++
    if(use_serial) {
        Serial.print("Cnt,\t");
    }
    
    if(use_xBee) {
        xBeeSerial.listen();
        xBeeSerial.print("Cnt,\t");
    }
    
    if(use_baro) {
        if(use_serial) {
            Serial.print("Alt,\tTemp,\t");
        }
        if(use_xBee) {
            xBeeSerial.print("Alt,\tTemp,\t");
        }
    }
    
    if(use_accel) {
        if(use_serial) {
            Serial.print("AccX,\tAccY,\tAccZ,\t");
        }
        if(use_xBee) {
            xBeeSerial.print("AccX,\tAccY,\tAccZ,\t");
        }
    }
    
    if(use_gyro) {
        if(use_serial) {
            Serial.print("GyroX,\tGyroY,\tGyroZ,\t");
        }
        if(use_xBee) {
            xBeeSerial.print("GyroX,\tGyroY,\tGyroZ,\t");
        }
    }
    
    if(use_mag) {
        if(use_serial) {
            Serial.print("MagX,\tMagY,\tMagZ,\t");
        }
        if(use_xBee) {
            xBeeSerial.print("MagX,\tMagY,\tMagZ,\t");
        }
    }
    
    if(use_servos) {
        if(use_serial) {
            Serial.print("Rudder,\tElevator,\tThrottle,\t");
        }
        if(use_xBee) {
            xBeeSerial.print("Rudder,\tElevatorVoltage,\tThrottle,\t");
        }
    }
    
    if(use_gps) {
        if(use_serial) {
            Serial.print("Lat,\tLon,\tAlt,\tCourse,\tSpeed, \tSats, \Error, ");
        }
        
        if(use_xBee) {
            xBeeSerial.print("Lat,\tLon,\tAlt,\tCourse,\tSpeed, \tSats, \Error, ");
        }
    }
    
    if(use_serial) {
        Serial.println();
    }
    
    if(use_xBee) {
        xBeeSerial.println();
    }
}

//------------------------------------------------------------------------------
// Loop Function - Arduino Program Loop
//------------------------------------------------------------------------------
void loop() {
    if(use_gps) {
        gps.listen();
    }
    
    digitalWrite(ledPin, HIGH);
    delay(ts-50);
    digitalWrite(ledPin, LOW);
    delay(50);
    
    // Declare local variables
    int accXYZ[3];
    int accX, accY, accZ;
    int gyroX, gyroY, gyroZ;
    int magX, magY, magZ;
    float lat, lon, alt, crs, mps, err;
    int nSats;
    int rudder, elevator, throttle;
    
    
    // Increment sample cnt
    cnt = cnt + 1;
    
    // GPS data
    if(use_gps) {
        gps.update();
        lat = gps.latitude();
        lon = gps.longitude();
        alt = gps.altitude();
        crs = gps.course();
        mps = gps.speed() * .514;
        nSats = gps.numberOfSats();
        err = gps.HDOP();
    }
    
    // Accelerometer data
    if(use_accel) {
        accel.readAccel(&accX, &accY, &accZ);
        accXYZ[0] = accX;
        accXYZ[1] = accY;
        accXYZ[2] = accZ;
    }

    // Gyro data
    if(use_gyro) {
        gyroX = gyro.x();
        gyroY = gyro.y();
        gyroZ = gyro.z();
    }

    // Magnetometer data
    if(use_mag) {
        mag.raw(magX, magY, magZ);
    }

    // Print data
    if(use_serial) {
        Serial.print(cnt);
        Serial.print(",\t");
    }
    
    if(use_xBee) {
        xBeeSerial.listen();
        xBeeSerial.print(cnt);
        xBeeSerial.print(",\t");
    }
    
    if(use_baro) {
        if(use_serial) {
            Serial.print(baro.altitude(),2); 
            Serial.print(",\t");
            Serial.print(baro.temperature(),2); 
            Serial.print(",\t");
        }
        
        if(use_xBee) {
            xBeeSerial.print(baro.altitude(),2); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(baro.temperature(),2); 
            xBeeSerial.print(",\t");
        }
    }

    if(use_accel) {
        if(use_serial) {
            Serial.print(accXYZ[0]); 
            Serial.print(",\t");
            Serial.print(accXYZ[1]); 
            Serial.print(",\t");
            Serial.print(accXYZ[2]); 
            Serial.print(",\t");
        }
        
        if(use_xBee) {
            xBeeSerial.print(accXYZ[0]); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(accXYZ[1]); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(accXYZ[2]); 
            xBeeSerial.print(",\t");
        }
    }
    
    if(use_gyro) {
        if(use_serial) {
            Serial.print(gyroX); 
            Serial.print(",\t");
            Serial.print(gyroY); 
            Serial.print(",\t");
            Serial.print(gyroZ); 
            Serial.print(",\t");
        }
        
        if(use_xBee) {
            xBeeSerial.print(gyroX); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(gyroY); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(gyroZ); 
            xBeeSerial.print(",\t");
        }
    }

    if(use_mag) {
        if(use_serial) {
            Serial.print(magX); 
            Serial.print(",\t");
            Serial.print(magY); 
            Serial.print(",\t");
            Serial.print(magZ); 
            Serial.print(",\t");
        }
        
        if(use_xBee) {
            xBeeSerial.print(magX); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(magY); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(magZ); 
            xBeeSerial.print(",\t");
        }
    }
    
    if(use_gps) {
        if(use_serial) {
            Serial.print(lat,5); 
            Serial.print(",\t");
            Serial.print(lon,5); 
            Serial.print(",\t");
            Serial.print(alt); 
            Serial.print(",\t");
            Serial.print(crs); 
            Serial.print(",\t");
            Serial.print(mps); 
            Serial.print(",\t");
            Serial.print(nSats);
            Serial.print(",\t");
            Serial.print(err);
            Serial.print(",\t");
        }

        if(use_xBee) {
            xBeeSerial.print(lat,5); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(lon,5); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(alt); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(crs); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(mps); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(nSats);
            xBeeSerial.print(",\t");
            xBeeSerial.print(err);
            xBeeSerial.print(",\t");
        }
    }

    if(use_servos) {
        rudder = analogRead(rudderAnalogIn) - rudderRef;
        elevator = analogRead(elevatorAnalogIn) - elevatorRef;
        throttle = throttleRef - analogRead(throttleAnalogIn);
        
        if(use_serial) {
            Serial.print(rudder); 
            Serial.print(",\t");
            Serial.print(elevator); 
            Serial.print(",\t");
            Serial.print(throttle);
            Serial.print(",\t");
        }
        
        if(use_xBee) {
            xBeeSerial.print(rudder); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(elevator); 
            xBeeSerial.print(",\t");
            xBeeSerial.print(throttle);
            xBeeSerial.print(",\t");
        }
    }

    if(use_serial) {
        Serial.println();
    }
    
    if(use_xBee) {
        xBeeSerial.println();
    }
}

//------------------------------------------------------------------------------
// Helper Functions
//------------------------------------------------------------------------------

